package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.spot.TelemetryServer;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.spot.DemoSwitchListener;
import com.grt192.event.component.spot.SwitchListener;
import com.grt192.mechanism.spot.SLight;
import com.grt192.sensor.spot.GRTCompass;
import com.grt192.sensor.spot.GRTDemoSwitch;
import com.grt192.sensor.spot.IDG500Gyro;
import com.grt192.utils.Util;
import com.iaroc.irobot.IRobotCreate;

/**
 *
 * @author ajc
 */
public class IRobotDriverSpot extends GRTObject implements Macro,
        TelemetryPorts,
        SensorChangeListener,
        DemoSwitchListener{

    public IRobotDriverSpot() {
        setPrinting(true);
    }

    public int execute() {
        
        while (true) {
            Util.sleep(1000);
        }
    }
    
    GRTDemoSwitch[] switches;
    public void startSwitch(){
        switches = new GRTDemoSwitch[2];
        for(int i = 0; i<switches.length; i++){
            switches[i] = new GRTDemoSwitch(0, 50, "Swtich" + 0);
            switches[i].addSwitchListener(this);
            switches[i].start();
        }
    }
    
    /*
     * 6 lights available:
     * compass, gyro, create
     * 1 for each
     */
    private SLight[] lights = SLight.get();
    private int compassLight = 3;
    private int gyroLight = 4;

    private void initLights() {
        lights[compassLight].rawColor(GRTDemoLED.Color.CHARTREUSE);
        lights[gyroLight].rawColor(GRTDemoLED.Color.CHARTREUSE);

    }
    private static final int RATE_PIN = 0;
    private static final int REF_PIN = 1;
    private static final int TEMP_PIN = 2;
    private static final double CHANGE_THRESHOLD = .5;
    IDG500Gyro gyro;
    GRTCompass compass;
    TelemetryServer forGyro;
    TelemetryServer forCompass;

    public void startSensors() {
        gyro = new IDG500Gyro(RATE_PIN, REF_PIN, TEMP_PIN, 20, "gyro");
        gyro.addSensorChangeListener(this);
        gyro.start();

        compass = new GRTCompass(CHANGE_THRESHOLD, 20, "compass");
        compass.addSensorChangeListener(this);
        compass.start();

        forGyro = new TelemetryServer(gyro, GYRO_PORT);
        forGyro.start();

        forCompass = new TelemetryServer(compass, COMPASS_PORT);
        compass.start();
    }

    public void sensorStateChanged(SensorEvent e, String key) {
        if (e.getSource() == gyro) {
            lights[compassLight].blinkToBlack();
        } else if (e.getSource() == compass) {
            log("compass heading " + e.getData(key));
            lights[gyroLight].blinkToBlack();

        }
    }

    public void switchPressed(GRTDemoSwitch source) {
    }

    public void switchReleased(GRTDemoSwitch source) {
    }
}
